// -*- coding: utf-8 -*-
// 8/5 7:15
/**
 * @file main.cpp
 * @brief Main entry point for drone control system
 */
#include "chassis.h"
#include "mission.h"
#include "commander.h"
#include <ros/ros.h>
#include <locale>
#include <codecvt>

int main(int argc, char **argv)
{
    // 设置中文环境
    std::setlocale(LC_ALL, "zh_CN.UTF-8");
    
    // 创建航路点管理器并设置初始任务
    WaypointManager waypointManager;
    
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;
    
    // 设置NodeHandle到WaypointManager中
    waypointManager.setNodeHandle(&nh);
    
    // 可以使用预定义的任务
    // createPatrolMissionWithPole(waypointManager); // 包含绕杆的巡逻任务
    createPatrolMissionWithTopic(waypointManager); // 包含话题控制的巡逻任务
    // createPoleSurroundTestMission(waypointManager); // 绕杆测试任务
    // 订阅命令消息

    // 订阅飞行器状态和发布位置设定点
    state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 10, state_cb);                     // 订阅状态
    pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, pose_cb); // 订阅位置信息.
    vel_sub = nh.subscribe<geometry_msgs::TwistStamped>("/mavros/local_position/velocity", 10, vel_cb);
    local_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 10);   // 设置位置
    local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10); // 设定速度
    set_raw_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);          // 以自身为坐标系的速度控制
    state_message_pub = nh.advertise<std_msgs::Int8>("/offboard_node/state", 10);
    // 定义服务客户端，用于解锁/上锁无人机和切换到离线控制模式
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
    ros::ServiceClient land_client = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/land");
    ros::ServiceServer state_client = nh.advertiseService("/command", state_cb);

    // 指定发布位置设定点的频率
    ros::Rate rate(50.0);

    // 等待 FCU 连接
    while (ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }

    // 发布一些起始位置设定点，然后才开始执行控制指令
    geometry_msgs::TwistStamped vector;
    // send a few setpoints before starting
    for (int i = 100; ros::ok() && i > 0; --i)
    {
        local_vel_pub.publish(vector);
        ros::spinOnce();
        rate.sleep();
    }

    // 请求进入离线控制模式
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    // 请求解锁飞行器
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 记录上一次请求时间
    ros::Time last_request = ros::Time::now();
    ros::Time last_time = ros::Time::now();
    ros::Time fire_time = ros::Time::now();
    mavros_msgs::CommandTOL land_cmd;
    std_msgs::Int8 offboard_state;
    std_msgs::Int8 drop_command;
    drop_command.data = 0;
    geometry_msgs::PoseStamped target_position;
    geometry_msgs::PoseStamped landing_position;
    geometry_msgs::PoseStamped rounding_pose[3];
    land_cmd.request.altitude = 0.0;
    land_cmd.request.yaw = 0.0;

    // 用于步骤
    int flag = 0;

    // 定义class的实例
    move_map move1;

    while (ros::ok())
    {
        ros::spinOnce();
        // ROS_INFO("Pose-z: %f",current_pose.pose.position.z);
        // 无人机状态设定与判断
        // 进入while循环后，先循环5s，然后再向客户端发送无人机状态设置的消息
        // set_mode_client.call   arming_client.call
        if (current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            {
                ROS_WARN("Offboard enabled");
            }
            last_request = ros::Time::now();
        }
        else
        {
            if (!current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0)))
            {
                if (arming_client.call(arm_cmd) &&
                    arm_cmd.response.success)
                {
                    ROS_WARN("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
            else //  无人机 Offboard enabled && Vehicle armed 后
            {
                // 使用增强版链表优化的航路点控制 - 统一任务管理
                static bool mission_initialized = false;

                // 一次性初始化任务系统
                if (!mission_initialized)
                {
                    flag = 0;
                    last_time = ros::Time::now();
                    mission_initialized = true;

                    // 开始第一个航路点任务
                    if (waypointManager.hasNextWaypoint())
                    {
                        Waypoint currentWP = waypointManager.getCurrentWaypoint();
                        ROS_WARN("Mission started! First task: %s (Action: %d)", currentWP.stepName.c_str(), currentWP.actionType);
                    }
                }

                // 执行航路点导航
                if (waypointManager.hasNextWaypoint())
                {
                    Waypoint currentWP = waypointManager.getCurrentWaypoint();

                    // 为话题控制设置订阅
                    if (currentWP.actionType == TOPIC_CONTROL)
                    {
                        waypointManager.setupTopicSubscription(nh, currentWP.topicName);
                    }

                    bool actionComplete = waypointManager.isCurrentActionComplete(move1, local_vel_pub, rate);

                    if (actionComplete)
                    {
                        last_time = ros::Time::now();
                        ROS_INFO("Completed %s", currentWP.stepName.c_str());

                        // 移动到下一个航路点
                        waypointManager.moveToNextWaypoint();

                        if (waypointManager.hasNextWaypoint())
                        {
                            Waypoint nextWP = waypointManager.getCurrentWaypoint();
                            ROS_INFO("Starting %s (Action: %d)", nextWP.stepName.c_str(), nextWP.actionType);
                            ROS_WARN("Starting %s (Action: %d)", nextWP.stepName.c_str(), nextWP.actionType);
                        }
                        else
                        {
                            // 所有航路点完成，任务结束
                            ROS_INFO("All waypoints completed, mission finished");
                            ROS_WARN("All waypoints completed, mission finished");
                        }
                    }
                }
                else
                {
                    ROS_WARN("No more waypoints, mission finished");
                }
            }
        }
        rate.sleep();
    }
    return 0;
}
